import time
import queue
import threading
import logging
import sys

sys.path.insert(0, "/usr/local/lib/python3.8/site-packages/cv2/python-3.8")
import cv2


class Camera:
    def __init__(self):
        self.run = False
        self.cap = None
        self.outqueue = queue.Queue(maxsize=10)
        self.logger = None
        self.__log_init()

    def __log_init(self):
        self.logger = logging.getLogger("Camera")

    def open(self, url):
        self.logger.trace("open.enter")
        self.cap = cv2.VideoCapture(url, cv2.CAP_GSTREAMER)
        if not self.cap.isOpened():
            self.logger.error("Failed to open camera")

        self.logger.trace("open.done")
        return self.cap.isOpened()

    def close(self):
        self.logger.trace("close.enter")
        if self.cap:
            self.cap.release()
        self.logger.trace("close.done")

    def thread(self):
        self.logger.trace("thread.enter")
        start_time = int(time.time() * 1000)

        number = 0
        while self.run:
            ret, frame = self.cap.read()
            timestamp = int(time.time() * 1000) - start_time

            if not ret:
                self.logger.error("Failed to read frame")
                time.sleep(0.1)
                continue

            data = {"timestamp": timestamp, "frame": frame}
            number += 1

            self.logger.info(f"Camera frame: {number}, timestamp: {timestamp}")

            try:
                if self.outqueue.full():
                    _ = self.outqueue.get_nowait()
                self.outqueue.put([data, number], timeout=0.1)
            except queue.Full:
                self.logger.warning("Queue full")
            except Exception as e:
                self.logger.error(f"Queue error: {e}")

        self.logger.trace("thread.done")

    def start(self):
        self.logger.trace("start.enter")

        if not self.cap or not self.cap.isOpened():
            self.logger.error("Camera not open")
            return False

        self.run = True
        self.handle = threading.Thread(target=self.thread)
        self.handle.start()

        self.logger.trace("start.done")
        return True

    def stop(self):
        self.logger.trace("stop.enter")
        self.run = False
        self.handle.join()
        self.logger.trace("stop.done")

    def output_queue_set(self, queue_):
        self.logger.trace("output_queue_set.enter")

        if self.run:
            self.logger.error("Camera is running, stop it before setting queue")
            return False

        self.outqueue = queue_
        self.logger.trace("output_queue_set.done")
        return True
